void Reset_ADXL345_DATA(void)
{
	ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAX0);
	ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAX1);
	ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAY0);
	ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAY1);
	ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAZ0);
	ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAZ1);
}

bool While_ADXL345_Ready(void)
{
	uint8_t dat=0;
	uint8_t i=0;
	
	for(i=0;i<10;i++)
	{
		dat=ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_INT_SOURCE);

		if((dat&0x80) && !(dat&0x01))
		{
			return true;
		}
		delay_ms(10);
		if((dat&0x01))
		{
			Reset_ADXL345_DATA();
		}
	}
	
	return false;
}
bool Wait_ADXL345_Ready(void)
{
	Reset_ADXL345_DATA();
	
	WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_POWER_CTL,0x08);
	delay_ms(10);
	if(While_ADXL345_Ready())
	{
//			Reset_ADXL345_DATA();
//		if(While_ADXL345_Ready())
//		{
			WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_POWER_CTL,0x00);
			return true;
//		}		
	}
	
	return false;
}

void Count_ADXL345(void)
{
	uint8_t dat_H=0;
	uint8_t dat_L=0;
	uint16_t dat=0;
	float x=0.0f;
	float y=0.0f;
	float z=0.0f;	

	uint8_t i=0;
	uint8_t Error_Flag=0;
	
	float x_sum=0.0f;
	float y_sum=0.0f;
	float z_sum=0.0f;
	
//	float x_min=0.0f;
//	float y_min=0.0f;
//	float z_min=0.0f;
//	
//	float x_max=0.0f;
//	float y_max=0.0f;
//	float z_max=0.0f;
	
	double x_2=0.0f;
	double y_2=0.0f;
	double z_2=0.0f;
	
	double x_rad=0.0f;
	double y_rad=0.0f;
	double z_rad=0.0f;
	
	float x_cir=0.0f;
	float y_cir=0.0f;
	float z_cir=0.0f;
	
	double pi=3.14159265358979;
	
	for(i=0;i<1;i++)
	{
		if(Wait_ADXL345_Ready())
		{
			dat_L = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAX0);
			dat_H = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAX1);
			dat = (dat_H<<8)|dat_L;
			x=((int16_t)dat)*0.00390625f;
			
			dat_L = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAY0);
			dat_H = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAY1);
			dat = (dat_H<<8)|dat_L;
			y=((int16_t)dat)*0.00390625f;	
			
			dat_L = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAZ0);
			dat_H = ReadOneByte((ADXL345_Slave_Add<<1),ADXL345_DATAZ1);
			dat = (dat_H<<8)|dat_L;
			z=((int16_t)dat)*0.00390625f;

			
			x_sum=x+x_sum;
			y_sum=y+y_sum;
			z_sum=z+z_sum;
		}
		else
		{
			Error_Flag++;
		}
	}
	
	if(Error_Flag>=1)
	{
		printf("[INFO] 	ADXL345 Error\n");
		return;
	}
	
	x=x_sum/(1.0f-Error_Flag);
	y=y_sum/(1.0f-Error_Flag);
	z=z_sum/(1.0f-Error_Flag);
	
	printf("[INFO] 	Ax: %0.4f	Ay: %0.4f	Az: %0.4f\n",x,y,z);
	
	x_2=pow(x,2);
	y_2=pow(y,2);
	z_2=pow(z,2);
	
	x_rad = atan(x/(sqrt(y_2+z_2)));
	y_rad = atan(y/(sqrt(x_2+z_2)));
	z_rad = atan(z/(sqrt(y_2+x_2)));
	
	x_cir=x_rad/pi*180;
	y_cir=y_rad/pi*180;
	z_cir=z_rad/pi*180;
	
	printf("[INFO] 	x: %0.4f	y: %0.4f	z: %0.4f\n",x_cir,y_cir,z_cir);	
	
	ADXL345_X_Stu.Measure_Now=x_cir;
	ADXL345_Y_Stu.Measure_Now=y_cir;
	ADXL345_Z_Stu.Measure_Now=z_cir;

	ADXL345_X_Stu=Kalman_Filter_Normal(ADXL345_X_Stu);
	ADXL345_Y_Stu=Kalman_Filter_Normal(ADXL345_Y_Stu);
	ADXL345_Z_Stu=Kalman_Filter_Normal(ADXL345_Z_Stu);
	
	printf("[INFO] 	Kx: %0.4f	Ky: %0.4f	Kz: %0.4f\n",ADXL345_X_Stu.Result_Now,ADXL345_Y_Stu.Result_Now,ADXL345_Z_Stu.Result_Now);	
}

void Init_ADXL345(void)
{
	HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8,GPIO_PIN_SET);
	
	WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_POWER_CTL,0x08);
	WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_DATA_FORMAT,0x00);
	WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_INT_MAP,0x80);
	WriteOneByte((ADXL345_Slave_Add<<1),ADXL345_INT_ENABLE,0x80);
	
	ADXL345_X_Stu.Q=0.1f;
	ADXL345_Y_Stu.Q=0.1f;
	ADXL345_Z_Stu.Q=0.1f;
	
	ADXL345_X_Stu.R=0.5f;
	ADXL345_Y_Stu.R=0.5f;
	ADXL345_Z_Stu.R=0.5f;
	
	
	ADXL345_X_Stu.Result_Last=0.0f;
	ADXL345_X_Stu.Prediction_Last=0.0f;
	ADXL345_X_Stu.Result_Now=0.0f;
	
	ADXL345_Y_Stu.Result_Last=0.0f;
	ADXL345_Y_Stu.Prediction_Last=0.0f;
	ADXL345_Y_Stu.Result_Now=0.0f;
	
	ADXL345_Z_Stu.Result_Last=90.0f;
	ADXL345_Z_Stu.Prediction_Last=90.0f;
	ADXL345_Z_Stu.Result_Now=90.0f;
}

